import os
import sys
import pygame
from pygame.locals import *
import time
import sys

sys.path.append('../external')
sys.path.append('../')

from quadtree import *
from dijkstra import *

__author__="mlopes"
__date__ ="$12/Nov/2009 11:49:26$"

### initialize pygame
pygame.init()

### BUG, display size should be read from the image size
imsize = [720, 576]
screen = pygame.display.set_mode(imsize)
background = pygame.image.load('LobstacleRobo.bmp').convert()
screen.blit(background, (0, 0))

### construct quadtree
qt = QuadTree( (0,0), imsize)

###if necessary read it from file
qt.fromfile()

###initialize pygame visualization for the quadtree
qt.setscreen( screen )



### check the cell number of the orig and dest coordinates
orig = qt.looknode( [0, 0])
dest = qt.looknode( [0, 0])

while 1:

    for event in pygame.event.get() :
      if event.type == pygame.MOUSEBUTTONDOWN :
        lb,mb,rb = pygame.mouse.get_pressed()
        ### convert mouse coordinates to make it compatible with RoboRealm
        pos = pygame.mouse.get_pos()
        mousepos = [ pos[0], imsize[1]-pos[1] ]
        if lb == 1:
            ### see the cell number of destiny and find shortest path
            dest = qt.looknode( mousepos )
            path = shortestPath( qt.G, orig[0], dest[0])
            print "plan", orig[0], dest[0], path
            orig = dest

            ### draw the path lines
            ### BUG: still dont know how to erase the lines afterwards
            for wp in range(0,len(path)-1):
                coord1 = qt.getcoordst(path[wp])
                coord2 = qt.getcoordst(path[wp+1])
                pygame.draw.line( screen, pygame.Color( 155, 155, 155, 1),\
                    [coord1[0], imsize[1]-coord1[1]],\
                    [coord2[0], imsize[1]-coord2[1]],\
                    4)

        if rb == 1:
            pygame.event.pump()
            m = pygame.key.get_mods()
            if m & KMOD_SHIFT:
                ### change property of a cell
                st, prop = qt.looknode( mousepos )
                qt.setprop( st, 1-prop[0] )
            else:
                ### add a new cell division
                qt.addnode( mousepos )
            ### update the distances map
            qt.updatedist()
            qt.tofile()
    
    bblist = qt.drawmqtree( )
    
    
    orig = dest
    pygame.display.update()
    pygame.time.delay(10)
